/*
# _____     ___ ____     ___ ____
#  ____|   |    ____|   |        | |____|
# |     ___|   |____ ___|    ____| |    \    PS2DEV Open Source Project.
#-----------------------------------------------------------------------
# Copyright 2005, James Lee (jbit<at>jbit<dot>net)
# Licenced under Academic Free License version 2.0
# Review ps2sdk README & LICENSE files for further details.
*/



/*
	Based on:
	PSX VAG-Packer, hacked by bITmASTER@bigfoot.com, hacked by jbit :)
	jbit's note:
*/

#include <math.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include "adpcm.h"

#define ADPCM_LOOP_START    4  /* Set on first block of looped data */
#define ADPCM_LOOP          2  /* Set on all blocks (?that are inside the loop?) */
#define ADPCM_LOOP_END      1  /* Set on last block to loop */

#define PACKED __attribute__((packed))

typedef struct
{
	unsigned shift:      4  PACKED;
	unsigned predict:    4  PACKED;
	uint8_t  flags;
	uint8_t  sample[14]; /* 4bits each */
} AdpcmBlock;

static void find_predict(AdpcmSetup *set, AdpcmBlock *adpcm, double *samples);
static void pack(AdpcmSetup *set, AdpcmBlock *adpcm, const double *samples);


AdpcmSetup *AdpcmCreate(AdpcmGetPCMfunc get, void *getpriv, AdpcmPutADPCMfunc put, void *putpriv, int loopstart)
{
	AdpcmSetup *set;

	set = malloc(sizeof(AdpcmSetup));
	if (set==NULL)
		return(NULL);

	set->s_1 = set->s_2 = 0.0;
	set->ps_1 = set->ps_2 = 0.0;

	set->curblock = 0;

	if (loopstart<0) /* disable looping (single shot) */
		set->loopstart = -1;
	else
		set->loopstart = loopstart;

	set->GetPCM = get;
	set->getpriv = getpriv;

	set->PutADPCM = put;
	set->putpriv = putpriv;
	set->pad = 0;
	return(set);
}

int AdpcmDestroy(AdpcmSetup *set)
{
	free(set);
	return(0);
}

int AdpcmEncode(AdpcmSetup *set, int blocks)
{
	AdpcmBlock adpcm;
	double samples[28];
	int procblocks;

	for (procblocks=0;procblocks<blocks;procblocks++)
	{
		int ret;
		adpcm.flags = 0;

		for (int j=0;j<28;j++)
			samples[j] = 0.0;

		ret = set->GetPCM(set->getpriv, samples, 28);
		if (ret<0)
			return(-1);
		if (ret<28)
		{
			printf("loop end!\n");
			adpcm.flags = ADPCM_LOOP_END;
		}

		if (set->loopstart>=0)
		{
			adpcm.flags |= ADPCM_LOOP;
			if (set->curblock == set->loopstart)
			{
				printf("loop start!\n");
				adpcm.flags |= ADPCM_LOOP_START;
			}
		}


		find_predict(set, &adpcm, samples);
		pack(set, &adpcm, samples);

		if (set->PutADPCM(set->putpriv, &adpcm, 16)<0)
			return(-1);

		set->curblock++;
		if (ret<28)
			break;
	}

	if (set->loopstart<0 && procblocks<blocks)
	{
		/* this block essentialy loops to itself and contains no data */
		adpcm.predict = 0;
		adpcm.shift = 0;
		adpcm.flags = ADPCM_LOOP_START | ADPCM_LOOP | ADPCM_LOOP_END;

		for (int i=0;i<14;i++)
			adpcm.sample[i] = 0;
		if (set->PutADPCM(set->putpriv, &adpcm, 16)<0)
			return(-1);
		set->curblock++;
	}

	if (procblocks<blocks && set->pad)
	{
		int padblocks = blocks-(set->curblock%blocks);

		adpcm.predict = 0;
		adpcm.shift = 0;
		adpcm.flags = ADPCM_LOOP_START | ADPCM_LOOP | ADPCM_LOOP_END;
		for (int i=0;i<14;i++)
			adpcm.sample[i] = 0;


		for (int i=0;i<padblocks;i++)
		{
			if (set->PutADPCM(set->putpriv, &adpcm, 16)<0)
			return(-1);
			set->curblock++;
		}
	}

	return(procblocks);
}

static double f[5][2] =
{
	{           0.0, 0.0},
   	{  -60.0 / 64.0, 0.0},
   	{ -115.0 / 64.0, 52.0 / 64.0},
   	{  -98.0 / 64.0, 55.0 / 64.0},
   	{ -122.0 / 64.0, 60.0 / 64.0}
};
#define MAX(_a,_b)   ((_a)>(_b) ? (_a):(_b))
#define MIN(_a,_b)   ((_a)<(_b) ? (_a):(_b))
#define CLAMP(_v,_min,_max) MIN(MAX(_v,_min),_max);

static void find_predict(AdpcmSetup *set, AdpcmBlock *adpcm, double *samples)
{
	double buffer[28][5];
	double min = 1e10;
	double max[5];
	double ds;
	int min2;
	int shift_mask;
	double s_0, s_1, s_2;

	for (int i=0;i<5;i++)
	{
		max[i] = 0.0;
		s_1 = set->s_1;
		s_2 = set->s_2;

		for (int j=0;j<28;j++)
		{
			s_0 = CLAMP(samples[j],-30720.0,30719.0);

			ds = s_0 + s_1 * f[i][0] + s_2 * f[i][1];
			buffer[j][i] = ds;
			if (fabs(ds) > max[i])
				max[i] = fabs(ds);

			s_2 = s_1;
			s_1 = s_0;
		}

		if ( max[i] < min )
		{
			min = max[i];
			adpcm->predict = i;
		}
		if (min <= 7)
		{
			adpcm->predict = 0;
			break;
		}
	}

	set->s_1 = s_1;
	set->s_2 = s_2;

	for (int  i=0;i<28;i++ )
		samples[i] = buffer[i][adpcm->predict];

	min2 = (int)min;
	shift_mask = 0x4000;
	adpcm->shift = 0;

	while(adpcm->shift < 12)
	{
		if (shift_mask & ( min2 + (shift_mask>>3) ))
			break;
		adpcm->shift++;
		shift_mask = shift_mask >> 1;
	}
}

static void pack(AdpcmSetup *set, AdpcmBlock *adpcm, const double *samples)
{
	double s_1, s_2;
	short four_bit[28];

	s_1 = set->ps_1;
	s_2 = set->ps_2;

	for (int i=0;i<28;i++)
	{
		double ds;
		int di;
		double s_0;

		s_0 = samples[i] + s_1 * f[adpcm->predict][0] + s_2 * f[adpcm->predict][1];
		ds = s_0 * (double) (1<<adpcm->shift);

		di = ((int)ds+0x800) & 0xfffff000;
		di = CLAMP(di,-32768,32767);

		four_bit[i] = (short)di;

		di = di >> adpcm->shift;
		s_2 = s_1;
		s_1 = (double) di - s_0;
	}

	for (int i=0;i<14;i++)
		adpcm->sample[i] = ( ( four_bit[(i*2)+1] >> 8 ) & 0xf0 ) | ( ( four_bit[i*2] >> 12 ) & 0xf );

	set->ps_1 = s_1;
	set->ps_2 = s_2;
}
